import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D

plt.rcParams['font.sans-serif'] = ['Microsoft YaHei']
plt.rcParams['axes.unicode_minus'] = False
plt.switch_backend('TkAgg')

# -------------------------- 1. 基础参数定义（严格遵循文档） --------------------------
target_true = np.array([0, 200, 0])  # 真目标下底面圆心（文档）
r, h = 7, 10  # 真目标：半径7m、高10m（文档）
target_fake = np.array([0, 0, 0])  # 假目标（原点，文档）

missile_M1 = np.array([20000, 0, 2000])  # M1初始位置（文档）
missile_speed = 300  # 导弹速度300m/s（文档）
drone_FY1 = np.array([17800, 0, 1800])  # FY1初始位置（文档）
drone_speed = 120  # FY1速度120m/s（问题1给定）

g = 9.8  # 重力加速度(m/s²)
smoke_radius = 10  # 烟幕有效半径10m（文档）
smoke_sink_speed = 3  # 烟幕下沉速度3m/s（文档）
smoke_valid_time = 20  # 烟幕起爆后20s有效（文档）
drone_drop_delay = 1.5  # 受领任务后1.5s投放（问题1给定）
smoke_detonate_delay = 3.6  # 投放后3.6s起爆（问题1给定）
detonate_t = drone_drop_delay + smoke_detonate_delay  # 烟幕起爆时刻（固定5.1s）


# -------------------------- 2. 真目标外接长方体顶点生成 --------------------------
def get_cylinder_bounding_box_vertices(target_center, radius, height):
    """生成真目标圆柱的外接长方体8个顶点（完全包裹圆柱）"""
    x_min = target_center[0] - radius
    x_max = target_center[0] + radius
    y_min = target_center[1] - radius
    y_max = target_center[1] + radius
    z_min = target_center[2]
    z_max = target_center[2] + height

    vertices = [
        np.array([x_min, y_min, z_min]),  # 前下左
        np.array([x_max, y_min, z_min]),  # 前下右
        np.array([x_max, y_max, z_min]),  # 后下右
        np.array([x_min, y_max, z_min]),  # 后下左
        np.array([x_min, y_min, z_max]),  # 前上左
        np.array([x_max, y_min, z_max]),  # 前上右
        np.array([x_max, y_max, z_max]),  # 后上右
        np.array([x_min, y_max, z_max])  # 后上左
    ]
    return vertices


# -------------------------- 3. 实体运动计算（修正爆炸点坐标） --------------------------
def missile_real_time_position(missile_init_pos, t):
    """计算t时刻导弹位置（指向假目标飞行）"""
    if t < 0:
        return missile_init_pos
    # 导弹方向向量（从初始位置指向假目标）
    dir_vec = target_fake - missile_init_pos
    dir_vec_mag = np.linalg.norm(dir_vec)
    dir_unit_vec = dir_vec / dir_vec_mag if dir_vec_mag > 1e-6 else np.array([0, 0, 0])
    # 实时位置
    displacement = missile_speed * t * dir_unit_vec
    return missile_init_pos + displacement


def get_smoke_explosion_center():
    """修正：计算烟幕起爆时刻的固定中心坐标（等高度飞行+平抛运动）"""
    # 1. 投放点坐标（无人机飞行1.5s到达）
    drop_x = drone_FY1[0] - drone_speed * drone_drop_delay  # x负方向飞行
    drop_y = drone_FY1[1]  # y不变（始终为0）
    drop_z = drone_FY1[2]  # 投放时z=1800（等高度飞行）

    # 2. 烟幕弹平抛运动（投放后3.6s起爆）
    explosion_x = drop_x - drone_speed * smoke_detonate_delay  # 水平速度不变
    explosion_y = drop_y  # y不变
    # 竖直自由下落：z = 投放z - 0.5*g*t²（t=3.6s）
    fall_distance = 0.5 * g * (smoke_detonate_delay ** 2)
    explosion_z = max(drop_z - fall_distance, 0.0)

    return np.array([round(explosion_x, 1), round(explosion_y, 1), round(explosion_z, 1)])


def smoke_real_time_center(t):
    """计算t时刻烟幕云团中心（起爆后匀速下沉）"""
    if t < detonate_t - 1e-6:
        return np.array([0, 0, 0])  # 起爆前无云团
    # 起爆时刻中心（固定）
    explosion_center = get_smoke_explosion_center()
    # 下沉距离 = 下沉速度 * (t - 起爆时刻)
    sink_distance = smoke_sink_speed * (t - detonate_t)
    # 实时z坐标（最低到地面z=0）
    real_z = max(explosion_center[2] - sink_distance, 0.0)
    return np.array([explosion_center[0], explosion_center[1], real_z])


# -------------------------- 4. 遮蔽判断（复用逻辑，修正云团位置） --------------------------
def is_segment_sphere_intersect(missile_pos, vertex_pos, smoke_center, smoke_radius):
    """判断“导弹-顶点视线线段”与“云团球体”是否相交"""
    vec_AC = smoke_center - missile_pos  # 导弹→云团中心
    vec_AB = vertex_pos - missile_pos  # 导弹→顶点（视线方向）

    ab_sq_mag = np.dot(vec_AB, vec_AB)
    if ab_sq_mag < 1e-12:  # 线段长度趋近于0，无意义
        return False

    # 参数k：定位线段上离云团最近的点
    k = np.dot(vec_AC, vec_AB) / ab_sq_mag

    # 计算最短距离
    if k <= 0.0:
        min_dist = np.linalg.norm(vec_AC)  # 最近点=导弹
    elif 0.0 < k < 1.0:
        # 最近点在线段内：叉积模长 / 线段长度
        cross_prod = np.cross(vec_AC, vec_AB)
        min_dist = np.linalg.norm(cross_prod) / np.sqrt(ab_sq_mag)
    else:
        vec_BC = smoke_center - vertex_pos  # 顶点→云团中心
        min_dist = np.linalg.norm(vec_BC)  # 最近点=顶点

    return min_dist <= smoke_radius + 1e-6  # 规避浮点误差


def is_effective_occlusion(t):
    """综合判断t时刻是否有效遮蔽（时间+空间双重验证）"""
    # 1. 时间有效性：仅[5.1, 25.1]s有效
    smoke_start = detonate_t
    smoke_end = detonate_t + smoke_valid_time
    if t < 0 or t < smoke_start - 1e-6 or t > smoke_end + 1e-6:
        return False

    # 2. 计算实时位置
    missile_pos = missile_real_time_position(missile_M1, t)
    target_vertices = get_cylinder_bounding_box_vertices(target_true, r, h)
    smoke_center = smoke_real_time_center(t)

    # 3. 只要1个顶点视线被挡，即有效遮蔽
    for vertex in target_vertices:
        if is_segment_sphere_intersect(missile_pos, vertex, smoke_center, smoke_radius):
            return True
    return False


# -------------------------- 5. 新增：有效遮蔽时长计算 --------------------------
def calculate_effective_duration(time_step=0.01):
    """计算总有效遮蔽时长（遍历时间区间，步长0.01s确保精度）"""
    smoke_start = detonate_t
    smoke_end = detonate_t + smoke_valid_time
    total_duration = 0.0
    # 记录上一时刻是否有效（判断连续区间）
    prev_effective = False

    # 遍历时间区间
    t = smoke_start
    while t <= smoke_end + 1e-6:
        current_effective = is_effective_occlusion(t)
        # 从无效变有效：记录起始时刻
        if current_effective and not prev_effective:
            start_t = t
        # 从有效变无效：累加时长
        elif not current_effective and prev_effective:
            total_duration += t - start_t
        # 更新状态
        prev_effective = current_effective
        t += time_step

    # 处理区间末尾仍有效的情况
    if prev_effective:
        total_duration += smoke_end - start_t

    return round(total_duration, 2)


# -------------------------- 6. 执行与结果输出 --------------------------
if __name__ == "__main__":
    # 1. 输出基础信息
    target_vertices = get_cylinder_bounding_box_vertices(target_true, r, h)
    vertex_names = ["前下左", "前下右", "后下右", "后下左", "前上左", "前上右", "后上右", "后上左"]
    print("=== 真目标外切长方体8顶点坐标 ===")
    for name, v in zip(vertex_names, target_vertices):
        print(f"{name}: ({v[0]}, {v[1]}, {v[2]}) m")

    explosion_center = get_smoke_explosion_center()
    print(f"\n=== 烟幕起爆时刻信息 ===")
    print(f"起爆时刻：{detonate_t} s")
    print(f"起爆点坐标：({explosion_center[0]}, {explosion_center[1]}, {explosion_center[2]}) m")
    print(f"烟幕有效时间区间：[{detonate_t}, {detonate_t + smoke_valid_time}] s")

    # 2. 计算并输出有效遮蔽时长
    effective_duration = calculate_effective_duration()
    print(f"\n=== 问题1结果 ===")
    print(f"烟幕对M1的有效遮蔽时长：{effective_duration} 秒")